#include "vehicle_linear_lateral.h"

using namespace auto_ros::control;
//VehicleLinearLv(double mass, double cf, double cr, double lf, double lr, double ix,
//double iz, double h, double k_phi, double d_phi, double max_lateral_acc = 9, double max_tire_f_angle = 0.5);
int main()
{
	//VehicleLinearLateral(double mass, double cf, double cr, double lf, double lr, double iz,
	//					 double max_lateral_acc = 9.0, double max_tire_f_angle = 0.5);
	VehicleLinearLateral test_vehicle_lateral(1600, 110000, 92000, 1.4, 1.65, 3234.0);

	std::cout << "state matrix is:" << std::endl
			  << test_vehicle_lateral.state_matrix() << std::endl;
	std::cout << "control matrix is:" << std::endl
			  << test_vehicle_lateral.control_matrix() << std::endl;
	std::cout << "distur matrix is:" << std::endl
			  << test_vehicle_lateral.distur_matrix() << std::endl;
	std::cout << "out state matrix is:" << std::endl
			  << test_vehicle_lateral.out_state_matrix() << std::endl;
	std::cout << "out distur matrix is:" << std::endl
			  << test_vehicle_lateral.out_distur_matrix() << std::endl;
	test_vehicle_lateral.speed_update_matrix(10.0);

	std::cout << "state matrix is:" << std::endl
			  << test_vehicle_lateral.state_matrix() << std::endl;
	std::cout << "control matrix is:" << std::endl
			  << test_vehicle_lateral.control_matrix() << std::endl;
	std::cout << "distur matrix is:" << std::endl
			  << test_vehicle_lateral.distur_matrix() << std::endl;
	std::cout << "out state matrix is:" << std::endl
			  << test_vehicle_lateral.out_state_matrix() << std::endl;
	std::cout << "out distur matrix is:" << std::endl
			  << test_vehicle_lateral.out_distur_matrix() << std::endl;
	std::shared_ptr<DiscreteLinearSys> test_lateral_dis_ptr = test_vehicle_lateral.discretization(0.01, "tustin");
	std::cout << "dis state matrix is:" << std::endl
			  << test_lateral_dis_ptr->state_matrix() << std::endl;
	std::cout << "dis control matrix is:" << std::endl
			  << test_lateral_dis_ptr->control_matrix() << std::endl;
	std::cout << "dis distur matrix is:" << std::endl
			  << test_lateral_dis_ptr->distur_matrix() << std::endl;
	std::cout << "dis out state matrix is:" << std::endl
			  << test_lateral_dis_ptr->out_state_matrix() << std::endl;
	std::cout << "dis out distur matrix is:" << std::endl
			  << test_lateral_dis_ptr->out_distur_matrix() << std::endl;
	/*	bool calc_steady_state_ctr(
		const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled,
		Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control);*/
	Eigen::VectorXd distur_ref(1);
	Eigen::VectorXd y_controlled_ref(test_lateral_dis_ptr->out_dim());
	Eigen::VectorXd steady_state(test_lateral_dis_ptr->state_dim());
	Eigen::VectorXd steady_control(test_lateral_dis_ptr->control_dim());
	distur_ref << 0.01;
	y_controlled_ref << 0.0;
	test_vehicle_lateral.calc_steady_state_ctr(distur_ref, y_controlled_ref, steady_state, steady_control);
	std::cout << "steady_state is:" << std::endl
			  << steady_state << std::endl;
	std::cout << "steady_control is:" << std::endl
			  << steady_control << std::endl;
}